home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / eigen / jacobi.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  7.5 KB  |  258 lines
  1. /* eigen/jacobi.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. /* Author:  G. Jungman
  21.  */
  22. /* Simple linear algebra operations, operating directly
  23.  * on the gsl_vector and gsl_matrix objects. These are
  24.  * meant for "generic" and "small" systems. Anyone
  25.  * interested in large systems will want to use more
  26.  * sophisticated methods, presumably involving native
  27.  * BLAS operations, specialized data representations,
  28.  * or other optimizations.
  29.  */
  30. #include <config.h>
  31. #include <stdlib.h>
  32. #include <string.h>
  33. #include <gsl/gsl_math.h>
  34. #include <gsl/gsl_vector.h>
  35. #include <gsl/gsl_matrix.h>
  36. #include <gsl/gsl_eigen.h>
  37.  
  38. #define REAL double
  39.  
  40. inline static void
  41. jac_rotate(gsl_matrix * a,
  42.            unsigned int i, unsigned int j, unsigned int k, unsigned int l,
  43.            double * g, double * h,
  44.            double s, double tau)
  45. {
  46.   *g = gsl_matrix_get(a, i, j);
  47.   *h = gsl_matrix_get(a, k, l);
  48.   gsl_matrix_set(a, i, j, (*g) - s*((*h) + (*g)*tau));
  49.   gsl_matrix_set(a, k, l, (*h) + s*((*g) - (*h)*tau));
  50. }
  51.  
  52. int
  53. gsl_eigen_jacobi(gsl_matrix * a,
  54.                          gsl_vector * eval,
  55.                          gsl_matrix * evec,
  56.                          unsigned int max_rot, 
  57.                          unsigned int * nrot)
  58. {
  59.   if(a->size1 != a->size2) {
  60.      GSL_ERROR ("eigenproblem requires square matrix", GSL_ENOTSQR);
  61.   }
  62.   else if(a->size1 != evec->size1 || a->size1 != evec->size2) {
  63.      GSL_ERROR ("eigenvector matrix must match input matrix", GSL_EBADLEN);
  64.   }
  65.   else if(a->size1 != eval->size) {
  66.     GSL_ERROR ("eigenvalue vector must match input matrix", GSL_EBADLEN);
  67.   }
  68.   else {
  69.     const unsigned int n = a->size1;
  70.     unsigned int i, j, iq, ip;
  71.     double t, s;
  72.  
  73.     REAL * b = (REAL *) malloc(n * sizeof(REAL));
  74.     REAL * z = (REAL *) malloc(n * sizeof(REAL));
  75.     if(b == 0 || z == 0) {
  76.       if(b != 0) free(b);
  77.       if(z != 0) free(z);
  78.       GSL_ERROR ("could not allocate memory for workspace", GSL_ENOMEM);
  79.     }
  80.  
  81.     /* Set eigenvectors to coordinate basis. */
  82.     for(ip=0; ip<n; ip++) {
  83.       for(iq=0; iq<n; iq++) {
  84.         gsl_matrix_set(evec, ip, iq, 0.0);
  85.       }
  86.       gsl_matrix_set(evec, ip, ip, 1.0);
  87.     }
  88.  
  89.     /* Initialize eigenvalues and workspace. */
  90.     for(ip=0; ip<n; ip++) {
  91.       REAL a_ipip = gsl_matrix_get(a, ip, ip);
  92.       z[ip] = 0.0;
  93.       b[ip] = a_ipip;
  94.       gsl_vector_set(eval, ip, a_ipip);
  95.     }
  96.  
  97.     *nrot = 0;
  98.  
  99.     for(i=1; i<=max_rot; i++) {
  100.       REAL thresh;
  101.       REAL tau;
  102.       REAL g, h, c;
  103.       REAL sm = 0.0;
  104.       for(ip=0; ip<n-1; ip++) {
  105.         for(iq=ip+1; iq<n; iq++) {
  106.           sm += fabs(gsl_matrix_get(a, ip, iq));
  107.         }
  108.       }
  109.       if(sm == 0.0) {
  110.         free(z);
  111.         free(b);
  112.         return GSL_SUCCESS;
  113.       }
  114.  
  115.       if(i < 4)
  116.         thresh = 0.2*sm/(n*n);
  117.       else
  118.         thresh = 0.0;
  119.  
  120.       for(ip=0; ip<n-1; ip++) {
  121.         for(iq=ip+1; iq<n; iq++) {
  122.           const REAL d_ip = gsl_vector_get(eval, ip);
  123.           const REAL d_iq = gsl_vector_get(eval, iq);
  124.       const REAL a_ipiq = gsl_matrix_get(a, ip, iq);
  125.           g = 100.0 * fabs(a_ipiq);
  126.           if(   i > 4
  127.              && fabs(d_ip)+g == fabs(d_ip)
  128.              && fabs(d_iq)+g == fabs(d_iq)
  129.          ) {
  130.             gsl_matrix_set(a, ip, iq, 0.0);
  131.           }
  132.           else if(fabs(a_ipiq) > thresh) {
  133.             h = d_iq - d_ip;
  134.             if(fabs(h) + g == fabs(h)) {
  135.               t = a_ipiq/h;
  136.             }
  137.             else {
  138.               REAL theta = 0.5*h/a_ipiq;
  139.               t = 1.0/(fabs(theta) + sqrt(1.0 + theta*theta));
  140.               if(theta < 0.0) t = -t;
  141.             }
  142.  
  143.             c   = 1.0/sqrt(1.0+t*t);
  144.             s   = t*c;
  145.             tau = s/(1.0+c);
  146.             h   = t * a_ipiq;
  147.             z[ip] -= h;
  148.             z[iq] += h;
  149.         gsl_vector_set(eval, ip, d_ip - h);
  150.         gsl_vector_set(eval, iq, d_iq + h);
  151.         gsl_matrix_set(a, ip, iq, 0.0);
  152.  
  153.             for(j=0; j<ip; j++){
  154.               jac_rotate(a, j, ip, j, iq, &g, &h, s, tau);
  155.             }
  156.             for(j=ip+1; j<iq; j++){
  157.               jac_rotate(a, ip, j, j, iq, &g, &h, s, tau);
  158.             }
  159.             for(j=iq+1; j<n; j++){
  160.               jac_rotate(a, ip, j, iq, j, &g, &h, s, tau);
  161.             }
  162.             for (j=0; j<n; j++){
  163.               jac_rotate(evec, j, ip, j, iq, &g, &h, s, tau);
  164.             }
  165.             ++(*nrot);
  166.           }
  167.         }
  168.       }
  169.       for (ip=0; ip<n; ip++) {
  170.         b[ip] += z[ip];
  171.         z[ip]  = 0.0;
  172.     gsl_vector_set(eval, ip, b[ip]);
  173.       }
  174.  
  175.       /* continue iteration */
  176.     }
  177.  
  178.     return GSL_EMAXITER;
  179.   }
  180. }
  181.  
  182. int
  183. gsl_eigen_invert_jacobi(const gsl_matrix * a,
  184.                              gsl_matrix * ainv,
  185.                              unsigned int max_rot)
  186. {
  187.   if(a->size1 != a->size2 || ainv->size1 != ainv->size2) {
  188.     return GSL_ENOTSQR;
  189.   }
  190.   else if(a->size1 != ainv->size2) {
  191.     return GSL_EBADLEN;
  192.   }
  193.   else {
  194.     const unsigned int n = a->size1;
  195.     unsigned int nrot;
  196.     unsigned int i,j,k,l;
  197.  
  198.     /* This is annoying because I do not want
  199.      * the error handling in these functions.
  200.      * But there are no "impl"-like versions
  201.      * of these allocators... sigh.
  202.      */
  203.     gsl_vector * eval = gsl_vector_alloc(n);
  204.     gsl_matrix * evec = gsl_matrix_alloc(n, n);
  205.     gsl_matrix * inv_diag = gsl_matrix_alloc(n, n);
  206.  
  207.     if(eval == 0 || evec == 0 || inv_diag == 0) {
  208.       if(eval != 0) gsl_vector_free(eval);
  209.       if(evec != 0) gsl_matrix_free(evec);
  210.       if(inv_diag != 0) gsl_matrix_free(inv_diag);
  211.       return GSL_ENOMEM;
  212.     }
  213.  
  214.     memcpy(ainv->data, a->data, n*n*sizeof(REAL));
  215.  
  216.     gsl_eigen_jacobi(ainv, eval, evec, max_rot, &nrot);
  217.  
  218.     for(i=0; i<n; i++) {
  219.       if(fabs(gsl_vector_get(eval, i)) < 100.0 * GSL_DBL_EPSILON) {
  220.         /* apparent singularity */
  221.         gsl_vector_free(eval);
  222.         gsl_matrix_free(evec);
  223.         gsl_matrix_free(inv_diag);
  224.         return GSL_ESING;
  225.       }
  226.     }
  227.  
  228.     /* Invert the diagonalized matrix. */
  229.     for(i=0; i<n; i++) {
  230.       for(j=0; j<n; j++) {
  231.         gsl_matrix_set(inv_diag, i, j, 0.0);
  232.       }
  233.       gsl_matrix_set(inv_diag, i, i, 1.0/gsl_vector_get(eval, i));
  234.     }
  235.  
  236.     for(i=0; i<n; i++) {
  237.       for(j=0; j<n; j++) {
  238.         gsl_matrix_set(ainv, i, j, 0.0);
  239.         for(k=0; k<n; k++) {
  240.           for(l=0; l<n; l++) {
  241.         REAL ainv_ij = gsl_matrix_get(ainv, i, j);
  242.         REAL evec_il = gsl_matrix_get(evec, i, l);
  243.         REAL evec_jk = gsl_matrix_get(evec, j, k);
  244.         REAL inv_diag_lk = gsl_matrix_get(inv_diag, l, k);
  245.         REAL delta = evec_il * inv_diag_lk * evec_jk;
  246.         gsl_matrix_set(ainv, i, j, ainv_ij + delta);
  247.       }
  248.     }
  249.       }
  250.     }
  251.  
  252.     gsl_vector_free(eval);
  253.     gsl_matrix_free(evec);
  254.     gsl_matrix_free(inv_diag);
  255.     return GSL_SUCCESS;
  256.   }
  257. }
  258.